#!/usr/bin/env python
# -*- coding:utf-8 -*-
# Ros libraries
import roslib
import rospy

import detector

# ros Messages
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Pose
from probot_vision.srv import *

rospy.init_node('Object_Detect')

image_params = rospy.get_param("~image")
red_params   = rospy.get_param("~redObj")
green_params = rospy.get_param("~greenObj")
blue_params  = rospy.get_param("~blueObj")
black_params = rospy.get_param("~blackObj")

rate = rospy.Rate(10) # 10 Hz

redObjList    = []
greenObjList  = []
blueObjList   = []
blackObjList  = []
res           = DetectObjectSrvResponse.NOT_DETECTED

while not rospy.is_shutdown():
    imageData = rospy.wait_for_message('/usb_cam/image_raw/compressed', CompressedImage, timeout=5)
    res, redObjList   = detector.execute(imageData.data, DetectObjectSrvRequest.RED_OBJECT,   red_params, image_params)
    res, greenObjList = detector.execute(imageData.data, DetectObjectSrvRequest.GREEN_OBJECT, green_params, image_params)
    res, blueObjList  = detector.execute(imageData.data, DetectObjectSrvRequest.BLUE_OBJECT,  blue_params, image_params)
    res, blackObjList = detector.execute(imageData.data, DetectObjectSrvRequest.BLACK_OBJECT, black_params, image_params)

    rate.sleep()
